
import os
import numpy as np
import cv2
import glob
import matchfunc
from miscfunc import *

##################################
##相机内参
K=np.array([[1.037575214664696e+03, 0.00000000e+00,         6.422315830312182e+02],
            [0.00000000e+00,        1.043315752317925e+03,  3.878357750962377e+02],
            [0.00000000e+00,        0.00000000e+00,         1.0]])

###############test
##参考坐标系在相机1的中心点
R1=np.eye(3)
C1=np.array([0,0,0])
R2,jac=cv2.Rodrigues(np.array([0,0,np.pi/6]) )      #相机2绕Z轴旋转30度，R2表示将相机2坐标系变换到参考坐标系
R2t=R2.transpose()      #R2t表示参考坐标系与相机2的变换
C2=np.array([0,0,1])    #相机2在参考坐标系中的位置
##skew_t=np.array([[0,-t2[2],t2[1]],[t2[2],0,-t2[0],],[-t2[1],t2[0],0]])

##3D点序列
pts3D=np.array([[33,31,23],[2,2,3],[5,3,7],[8,7,2],[2,4,9],[10,6,10],[20,11,8],[-1,20,15],[-15,4,20],[20,-4,9],[22,-7,34],[22,8,13],[5,5,21],[6,10,9],[53,32,19]], dtype=np.float)
##得到投影图像点
P1=np.hstack((R1,C1.reshape([3,1])))
nRC2=-np.dot(R2t,C2.reshape([3,1]))
P2=np.hstack((R2t,nRC2))

print("原始R值："); print(R2t)
print("原始t值："); print(nRC2)

rvec, jacobi = cv2.Rodrigues(P1[:3, :3])
tvec = P1[:, 3]
img_pts1,jacobi=cv2.projectPoints(pts3D,rvec,tvec,K,None)
img_pts1=img_pts1[:,0,:]

rvec, jacobi = cv2.Rodrigues(P2[:3, :3])
tvec = P2[:, 3]
img_pts2,jacobi=cv2.projectPoints(pts3D,rvec,tvec,K,None)
img_pts2=img_pts2[:,0,:]

##计算F矩阵
F, mask = cv2.findFundamentalMat(img_pts1, img_pts2, cv2.FM_RANSAC, 1, 0.8)
#print(F)
goodPts1 = np.array([img_pts1[i] for i in range(len(img_pts1)) if mask[i]])
goodPts2 = np.array([img_pts2[i] for i in range(len(img_pts2)) if mask[i]])
R, t, ratio = matchfunc.estimateRelativePose(goodPts1, goodPts2, K, F)
if ratio < 0.8:
    print("Pose of view can't be found!")
    exit(1)
print("估计的R值："); print(R)
print("估计的t值："); print(t)

exit(0)